#include"KalmanTracker.hpp"
// 初始化卡尔曼调用对象
void KalmanTracker::inital_KalmanTracker(const Mat &rvec,const Mat &tvec){
    if(this->isFirst){ // 如果追踪器是第一次生成的就开辟新的空间，相当于初始化
        this->klc = new KalmanCalculate(rvec,tvec);
        this->isFirst = false;  // 标记已初始化
    }
}

// 卡尔曼更新操作
void KalmanTracker::update_KalmanTracker(const KalmanTracker &tracker){
    this->frameArmor = tracker.frameArmor;
    this->frameNumber = tracker.frameNumber; 
    this->frameLostCount = 3; 
}



// 卡尔慢滤波 调用函数
void KalmanTracker::CumulateKalman(){
    
    this->klc->updateKalmanTracker(tools::TimeStep); // 更新卡尔曼滤波状态

    // 第一次点位的处理
    if(klc->isFirstFrame){
        klc->init(this->frameArmor.rvec,this->frameArmor.tvec);// 后验状态的起点
        klc->isFirstFrame = false;
        return;
    }
    // 预测
    this->klc->kf.predict();

    // 2、结合观测对先验状态进行修正,并经过一系列计算得到后验状态
    this->klc->correct(this->frameArmor.rvec,this->frameArmor.tvec); // 结合先验与状态矩阵得到最优估计

    // 引用的方式返回
    this->p_tvec = this->frameArmor.tvec + this->klc->getTVelocity() * 1.65;
    this->p_rvec = this->frameArmor.rvec + this->klc->getRVelocity() * 1.65; 
    // this->p_tvec =  this->klc->getTvec() + this->klc->getTVelocity() * 3; 
    // this->p_rvec =  this->klc->getRvec() + this->klc->getRVelocity() * 3;

    this->distance = this->p_tvec.at<double>(2); // 距离 
    
}


